import rclpy
from rclpy.node import Node
from topic_msg.msg import CatVision
from topic_msg.msg import CatStatus
from topic_msg.msg import CatSteer
from topic_msg.msg import CatDeriction
from topic_msg.msg import CatHearing
from topic_msg.msg import CatSpeak
import threading
import time
from .angle import Angle
from .angle import generate_trail
import random


class CatNeckNode(Node):
    ANGLE_X_K = 0.06
    ANGLE_Y_K = 0.06

    def __init__(self, name):
        super().__init__(name)
        self._angle_lock = threading.Lock()
        self._angle = Angle(Angle.MID_X, Angle.MID_Y)
        self._cat_deriction_lock = threading.Lock()
        self._cat_deriction = CatDeriction.HORIZONTAL
        self._trail_lock = threading.Lock()
        self._trail = []

        time.sleep(4)

        self._cat_deriction_subscriper = self.create_subscription(
            CatDeriction, "cat_deriction", self._rcv_cat_deriction_callback, 10
        )
        self._cat_vision_subscriper = self.create_subscription(
            CatVision, "cat_vision", self._rcv_cat_vision_callback, 10
        )
        self._hearing_subscriper = self.create_subscription(
            CatHearing, "cat_hearing", self._rcv_hearing_callback, 10
        )

        self._speak_publisher = self.create_publisher(CatSpeak, "cat_speak", 10)

        self._neck_publisher = self.create_publisher(CatSteer, "cat_steers", 10)

        self._set_cat_neck_angle()

        self._thd_deal_trail = threading.Thread(target=self._deal_trail)
        self._thd_deal_trail.start()

        self._thd_random_shake = threading.Thread(target=self._random_shake)
        self._thd_random_shake.start()

    def __del__(self):
        self._thd_deal_trail.join()
        self._thd_random_shake.join()

        self._hearing_subscriper.destroy()
        self._cat_status_subscriper.destroy()
        self._cat_vision_subscriper.destroy()
        self._cat_deriction_subscriper.destroy()
        self._neck_publisher.destroy()
        self._speak_publisher.destroy()

    def _rcv_hearing_callback(self, msg):
        with self._trail_lock:
            if len(self._trail) != 0:
                return
        with self._trail_lock:
            self._trail.clear()
            if msg.cmd == CatHearing.LOOK_UP_1 or msg.cmd == CatHearing.LOOK_UP_2:
                self._look_up()
            elif msg.cmd == CatHearing.LOOK_DOWN_1 or msg.cmd == CatHearing.LOOK_DOWN_2:
                self._look_down()
            elif msg.cmd == CatHearing.LOOK_LEFT:
                self._look_left()
            elif msg.cmd == CatHearing.LOOK_RIGHT:
                self._look_right()
            elif msg.cmd == CatHearing.SHAKE_HEAD:
                self._shake_head()
            else:
                self._random_angle()

    def _look_up(self):
        tmp = Angle(Angle.MID_X, Angle.MID_Y)
        with self._angle_lock:
            tmp = self._angle
        angles = generate_trail(tmp, Angle(Angle.MID_X, Angle.MAX_Y))
        self._trail.extend(angles)

    def _look_down(self):
        tmp = Angle(Angle.MID_X, Angle.MID_Y)
        with self._angle_lock:
            tmp = self._angle
        angles = generate_trail(tmp, Angle(Angle.MID_X, Angle.MIN_Y))
        self._trail.extend(angles)

    def _look_left(self):
        tmp = Angle(Angle.MID_X, Angle.MID_Y)
        with self._angle_lock:
            tmp = self._angle
        angles = generate_trail(tmp, Angle(Angle.MAX_X, Angle.MID_Y))
        self._trail.extend(angles)

    def _look_right(self):
        tmp = Angle(Angle.MID_X, Angle.MID_Y)
        with self._angle_lock:
            tmp = self._angle
        angles = generate_trail(tmp, Angle(Angle.MIN_X, Angle.MID_Y))
        self._trail.extend(angles)

    def _shake_head(self):
        tmp = Angle(Angle.MID_X, Angle.MID_Y)
        with self._angle_lock:
            tmp = self._angle
        i = 0
        while i < 2:
            end_angle = Angle(Angle.MID_X + 15, Angle.MID_Y)
            angles = generate_trail(tmp, end_angle)
            self._trail.extend(angles)
            tmp = end_angle
            end_angle = Angle(Angle.MID_X - 15, Angle.MID_Y)
            angles = generate_trail(tmp, end_angle)
            self._trail.extend(angles)
            tmp = end_angle
            i += 1

    def _rcv_cat_deriction_callback(self, msg):
        with self._cat_deriction_lock:
            self._cat_deriction = msg.deriction

    def _random_shake(self):
        while rclpy.ok():
            time.sleep(0.5)
            with self._trail_lock:
                if len(self._trail) != 0:
                    continue
                else:
                    i = random.randint(0, 5)
                    if i == 1:
                        self._random_angle()
                    elif i == 2 or i == 3:
                        self._return_center()
                    else:
                        pass

    def _deal_trail(self):
        while rclpy.ok():
            time.sleep(0.02)
            with self._trail_lock:
                if len(self._trail) == 0:
                    continue
                else:
                    with self._angle_lock:
                        self._angle = self._trail[0]
                        del self._trail[0]
                    self._set_cat_neck_angle()

    def _rcv_cat_vision_callback(self, msg):
        if self._no_object(msg) is False:
            self._random_announce()
            self._set_trail_face(msg)

    def _random_announce(self):
        msg = CatSpeak()
        msg.announce = CatSpeak.USUAL
        if random.randint(1, 500) % 10 == 1:
            self._speak_publisher.publish(msg)

    def _set_trail_face(self, msg):
        des = self._caculate_angle_to_face(msg)

        tmp = Angle(Angle.MID_X, Angle.MID_Y)
        with self._angle_lock:
            tmp = self._angle

        angles = generate_trail(tmp, des)

        with self._trail_lock:
            self._trail.clear()
            self._trail.extend(angles)

    def _no_object(self, msg):
        if msg.number == 0:
            return True
        else:
            if msg.number != len(msg.x) or msg.number != len(msg.y):
                return True
            else:
                return False

    def _return_center(self):
        tmp = CatDeriction.HORIZONTAL
        angle = Angle(Angle.MID_X, Angle.MID_Y)

        with self._cat_deriction_lock:
            tmp = self._cat_deriction

        if tmp != CatDeriction.HORIZONTAL:
            angle.y = Angle.MIN_Y

        with self._angle_lock:
            cur_angle = self._angle

        angles = generate_trail(cur_angle, angle)

        self._trail.clear()
        self._trail.extend(angles)

    def _random_angle(self):
        tmp = CatDeriction.HORIZONTAL
        angle = Angle(Angle.MID_X, Angle.MID_Y)

        with self._cat_deriction_lock:
            tmp = self._cat_deriction

        if tmp != CatDeriction.HORIZONTAL:
            angle.y = Angle.MIN_Y

        angle.x = random.randint(Angle.MIN_X + 20, Angle.MAX_X - 20)

        if self._cat_deriction == CatDeriction.HORIZONTAL:
            angle.y = random.randint(Angle.MIN_Y + 20, Angle.MAX_Y - 20)
        else:
            angle.y = random.randint(Angle.MIN_Y, Angle.MID_Y)

        with self._angle_lock:
            cur_angle = self._angle

        angles = generate_trail(cur_angle, angle)

        self._trail.clear()
        self._trail.extend(angles)

    def _caculate_angle_to_face(self, msg):
        viosion_x = 0
        viosion_y = 0
        for i in range(msg.number):
            viosion_x += msg.x[i]
            viosion_y += msg.y[i]

        viosion_x = int(viosion_x / msg.number)
        viosion_y = int(viosion_y / msg.number)

        err_x = viosion_x - int(msg.width / 2)
        err_y = viosion_y - int(msg.height / 2)

        angle_x = int(self._angle.x - err_x * CatNeckNode.ANGLE_X_K)
        angle_y = int(self._angle.y - err_y * CatNeckNode.ANGLE_Y_K)

        return Angle(angle_x, angle_y)

    def _deal_out_range_angle(self):
        if self._angle.x > Angle.MAX_X:
            self._angle.x = Angle.MAX_X
        elif self._angle.x < Angle.MIN_X:
            self._angle.x = Angle.MIN_X

        if self._angle.y > Angle.MAX_X:
            self._angle.y = Angle.MAX_X
        elif self._angle.y < Angle.MIN_Y:
            self._angle.y = Angle.MIN_Y

    def _set_cat_neck_angle(self):
        self._deal_out_range_angle()
        msg = CatSteer()
        msg.number = CatSteer.NECK_STERR_NUMBER
        msg.type = CatSteer.NECK_TYPE
        msg.steer.append(CatSteer.NECK_LEFT_RIGHT)
        msg.steer.append(CatSteer.NECK_UP_DOWN)
        msg.angle.append(int(self._angle.x))
        msg.angle.append(int(self._angle.y))
        self._neck_publisher.publish(msg)


def main():
    rclpy.init()
    node = CatNeckNode("cat_neck")
    rclpy.spin(node)
    rclpy.shutdown()
